#include "positionsensor.h"
#include <QDebug>

int PositionSensor::counter = 0;

PositionSensor::PositionSensor(AbstractSystem *a_sys, int freq, double a_delay, double a_imprecision):
    sys(a_sys), imprecision(a_imprecision)
{
    dt = 1./freq;

    delay = static_cast<int>(a_delay*freq);

    QList<double> list;

    for(int i=0; i<delay; i++)
        storedValues << list;


    timer = new QTimer;
    timer->setInterval(static_cast<int>(1000*dt));
    timer->start();

    values << 0 << 0;

    connect(timer,SIGNAL(timeout()), this, SLOT(update()));
}

void PositionSensor::update()
{
    QList<double> newValues= sys->getPositionNormee();
    //TODO gaussian noise on result
    if(delay>0){
        values = storedValues[counter];
        storedValues[counter] = newValues;
        counter= (counter+1)%delay;
    }
    else
        values = newValues;

    //TODO gaussian noise on result

}

QList<double> PositionSensor::getValues()
{
    return(values);
}
